package org.example.GirdFind;

import org.example.bean.Cord;
import org.example.bean.JsonGamePos;

import javax.swing.*;
import java.util.*;

/**
 * @ClassName GirdThetaStarUtil
 * @Description TODO
 * @Author hongjun
 * @Date 2024/5/26 下午6:39
 * @Version 1.0
 */
public class GirdThetaStarUtil {

    private static final int LINEAR_COST = 10;

    private static final int DIAG_COST = 14; // sqrt(2), 代表对角线移动的成本


    /**
     * 大格子寻路
     *
     * @param start   起点
     * @param end     终点
     * @param gameMap 地图数据
     * @return 从起点到终点的路径，若无法到达则返回null
     */
    public static List<JsonGamePos> findPathGird(JsonGamePos start, JsonGamePos end, GameGirdMap gameMap) {
        long now = System.currentTimeMillis();
        int maxsize = 1200 * 1200;
        GirdNode startNode = gameMap.JsonGamePosToMapGirdIndex(start);
        GirdNode goalNode = gameMap.JsonGamePosToMapGirdIndex(end);
        PriorityQueue<GirdNode> openSet = new PriorityQueue<>(Comparator.comparingDouble(GirdNode::getfScore));
        HashSet<GirdNode> closedSet = new HashSet<>();
        List<Gird> list = new ArrayList<>();
        startNode.setgScore(0);
        startNode.sethScore(heuristic(startNode, goalNode));
        startNode.setfScore(startNode.getgScore() + startNode.gethScore());
        openSet.add(startNode);
        while (!openSet.isEmpty()) {
            if (openSet.size() > maxsize) {
                System.out.println("findPathBigGird openSet exceed openSet.size = " + openSet.size());
                //画图
                List<Cord> list2 = new ArrayList<>();
                for (GirdNode girdNode : closedSet) {
                    list2.add(new Cord(girdNode.getX(), girdNode.getY()));
                }
                List<Cord> list1 = new ArrayList<>();
                list1.add(new Cord((int) start.getX(), (int) start.getZ()));
                list1.add(new Cord((int) end.getX(), (int) end.getZ()));
                SwingUtilities.invokeLater(() -> {
                    GirdMapDrawing gameDraw = new GirdMapDrawing(gameMap, list1, list2, "分层寻路");
                    gameDraw.setVisible(true);
                });
                return null;
            }
            GirdNode currentNode = openSet.poll();
            currentNode.setInOpenSet(false);
            if (currentNode.equals(goalNode)) {
                list = gameMap.reconstructGird(currentNode);
                break;
            }
            closedSet.add(currentNode);
            List<Gird> girdNeighbors = gameMap.getGirdNeighbors(currentNode);
            for (Gird gird : girdNeighbors) {
                if (gird.isStopBlock()) {
                    continue;
                }
                GirdNode neighbor = new GirdNode(gird.getX(), gird.getY());
                if (closedSet.contains(neighbor) || openSet.contains(neighbor)) {
                    continue;
                }
                int tentativeGScore = currentNode.getgScore() + distBetween(currentNode, neighbor);
                //if (gird.getType() == 0) {
                //    tentativeGScore += gird.getBlock();
                //}
                if (!neighbor.isInOpenSet() || tentativeGScore < neighbor.getgScore()) {
                    neighbor.setParent(currentNode);
                    neighbor.setgScore(tentativeGScore);
                    neighbor.sethScore(heuristic(neighbor, goalNode));
                    neighbor.setfScore(neighbor.getgScore() + neighbor.gethScore());
                    openSet.add(neighbor);
                    neighbor.setInOpenSet(true);
                }
            }
        }
        System.out.println("大格子寻路耗时：" + (System.currentTimeMillis() - now));
        List<JsonGamePos> pathGird = new ArrayList<>();
        for (Gird gird : list) {
            pathGird.add(new JsonGamePos(gird.getX(), gird.getY()));
        }
        //List<JsonGamePos> pathGird = refinePathGird(list, gameMap, start, end);
        return pathGird;
    }

    /**
     * 大格子寻路的路径上进行细化，阻挡为GameGirdMap的blocks
     *
     * @return 从起点到终点的路径，若无法到达则返回null
     */
    public static List<JsonGamePos> refinePathGird(List<Gird> list, GameGirdMap gameMap, JsonGamePos start, JsonGamePos end) {
        List<JsonGamePos> jsonGamePos = new ArrayList<>();
        long now = System.currentTimeMillis();
        jsonGamePos.add(start);
        Gird lastGird = null;
        for (int i = 0; i < list.size(); i++) {
            Gird gird = list.get(i);
            //格子中心坐标
            int x = gird.getX() * 10 + 5;
            int y = gird.getY() * 10 + 5;
            JsonGamePos gamePos = new JsonGamePos(x / 10f, y / 10f);
            //if (gird.getBlock() == 0 && lastGird != null && lastGird.getBlock() == 0) {
            //jsonGamePos.add(gamePos);
            //} else {
            JsonGamePos lastPos = jsonGamePos.get(jsonGamePos.size() - 1);
            if (!gameMap.isBlocked(x, y)) {
                List<JsonGamePos> path = findPath(lastPos, gamePos, gameMap);
                jsonGamePos.addAll(path);
            } else {
                System.out.println("error");
            }
            //}
            lastGird = gird;
        }
        JsonGamePos jsonGamePos1 = jsonGamePos.get(jsonGamePos.size() - 1);
        List<JsonGamePos> path = findPath(jsonGamePos1, end, gameMap);
        jsonGamePos.addAll(path);
        jsonGamePos = gameMap.optimizeJsonGamePosPath(jsonGamePos);
        System.out.println("小格子寻路耗时：" + (System.currentTimeMillis() - now) + "  路径点：" + jsonGamePos.size());
        return jsonGamePos;
    }


    /**
     * 使用Theta*算法找到从起点到终点的路径。
     *
     * @param start   起点
     * @param end     终点
     * @param gameMap 地图数据
     * @return 从起点到终点的路径，若无法到达则返回null
     */
    public static List<JsonGamePos> findPath(JsonGamePos start, JsonGamePos end, GameGirdMap gameMap) {
        //logger.info("ThetaStarUtil findPath startNode :{} goalNode:{}", start, end);
        GirdNode startNode = gameMap.gameToMapIndex(start.getX(), start.getZ());
        GirdNode goalNode = gameMap.gameToMapIndex(end.getX(), end.getZ());
        long now = System.currentTimeMillis();
        PriorityQueue<GirdNode> openSet = new PriorityQueue<>(Comparator.comparingDouble(GirdNode::getfScore));
        HashSet<GirdNode> closedSet = new HashSet<>();
        List<JsonGamePos> jsonGamePos = new ArrayList<>();
        startNode.setgScore(0);
        startNode.sethScore(heuristic(startNode, goalNode));
        startNode.setfScore(startNode.getgScore() + startNode.gethScore());
        openSet.add(startNode);

        while (!openSet.isEmpty()) {
            if (openSet.size() >= 1000) {
                break;
            }
            GirdNode currentNode = openSet.poll();
            currentNode.setInOpenSet(false);

            if (currentNode.equals(goalNode)) {
                List<Cord> lazyNodes = reconstructPath(currentNode);
                //logger.info("ThetaStarUtil 寻路耗时 ：{} ", (System.currentTimeMillis() - now));
                now = System.currentTimeMillis();
                jsonGamePos = gameMap.optimizePath(lazyNodes);
                //logger.info("ThetaStarUtil 平滑路径耗时：{} 平滑前节点:{} 平滑后节点:{}", (TimeUtil.now() - now), lazyNodes.size(), jsonGamePos.size());
                break;
            }

            closedSet.add(currentNode);

            for (GirdNode neighbor : gameMap.getNeighbors(currentNode)) {
                if (closedSet.contains(neighbor)) {
                    continue;
                }
                int tentativeGScore = currentNode.getgScore() + distBetween(currentNode, neighbor);
                if (!neighbor.isInOpenSet() || tentativeGScore < neighbor.getgScore()) {
                    neighbor.setParent(currentNode);
                    neighbor.setgScore(tentativeGScore);
                    neighbor.sethScore(heuristic(neighbor, goalNode));
                    neighbor.setfScore(neighbor.getgScore() + neighbor.gethScore());
                    openSet.add(neighbor);
                    neighbor.setInOpenSet(true);
                }
            }
        }
        //释放节点回池中
        for (GirdNode node : closedSet) {
            if (gameMap.getNodePool().release(node)) {
                break;
            }
        }
        for (GirdNode node : openSet) {
            if (gameMap.getNodePool().release(node)) {
                break;
            }
        }
        return jsonGamePos;
    }


    /**
     * 根据给定的当前节点，重建并返回从起点到该节点的路径。
     *
     * @param currentNode 当前节点
     * @return 从起点到当前节点的路径
     */
    private static List<Cord> reconstructPath(GirdNode currentNode) {
        List<Cord> path = new ArrayList<>();
        int depth = 0;
        while (currentNode != null) {
            if (depth == 0 || depth != currentNode.getDirection() || currentNode.getParent() == null) {
                path.add(new Cord(currentNode.getX(), currentNode.getY()));
                depth = currentNode.getDirection();
            }
            currentNode = currentNode.getParent();
        }
        Collections.reverse(path);
        return path;
    }


    /**
     * 计算两个节点之间的启发式距离。曼哈顿距离距离
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 启发式距离
     */
    private static int heuristic(GirdNode nodeA, GirdNode nodeB) {
        return (Math.abs(nodeA.getX() - nodeB.getX()) + Math.abs(nodeA.getY() - nodeB.getY())) ;
    }

    /**
     * 计算两个相邻节点的移动代价。
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 两点之间的距离
     */
    private static int distBetween(GirdNode nodeA, GirdNode nodeB) {
        int dx = Math.abs(nodeA.getX() - nodeB.getX());
        int dy = Math.abs(nodeA.getY() - nodeB.getY());

        if (dx == 0 || dy == 0) {
            //直线
            return LINEAR_COST;
        } else {
            //对角线
            return DIAG_COST;
        }
    }
}
